/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "Transformable.h"

using std::string;

Transformable::Transformable() {
	x = 0;
	y = 0;
	z = 0;
}

void Transformable::setPosition(string pos) {
	sscanf(pos.c_str(), "%f %f %f", &x, &y, &z);
}

void Transformable::setPosition(float x, float y, float z) {
	this->x = x;
	this->y = y;
	this->z = z;
}

void Transformable::setPosition(Vector3D pos) {
	x = pos.x;
	y = pos.y;
	z = pos.z;
}

float Transformable::getX() {
	return x;
}

float Transformable::getY() {
	return y;
}

float Transformable::getZ() {
	return z;
}

Vector3D Transformable::getPosition() {
	return Vector3D(x,y,z);
}

void Transformable::setRotation(string pos) {
	float roll, pitch, yaw;
	sscanf(pos.c_str(), "%f %f %f", &roll, &pitch, &yaw );
	rotation.setRpy(roll, pitch, yaw);
}	

void Transformable::setRotation(float roll, float pitch, float yaw) {
	rotation.setRpy(roll, pitch, yaw);
}

void Transformable::setRotation(Quaternion q) {
	rotation.w = q.w;
	rotation.x = q.x;
	rotation.y = q.y;
	rotation.z = q.z;
}

Quaternion& Transformable::getRotation() {
	return rotation;
}

float Transformable::getYaw() {
	return rotation.getYaw();
}

float Transformable::getPitch() {
	return rotation.getPitch();
}

float Transformable::getRoll() {
	return rotation.getRoll();
}
